package com.crunch.math;

import java.util.Arrays;

/**
 * A class for 4x4 affine matrices.
 */
public class Matrix44af {
	float[] d;

	/**
	 * Constructs an identity matrix.
	 */
	public Matrix44af() {
		d = new float[] { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f };
	}

	/**
	 * Sets this matrix to the identity matrix.
	 */
	public void setIdentity() {
		d[ 0] = 1.0f;
		d[ 1] = 0.0f;
		d[ 2] = 0.0f;
		d[ 3] = 0.0f;
		d[ 4] = 0.0f;
		d[ 5] = 1.0f;
		d[ 6] = 0.0f;
		d[ 7] = 0.0f;
		d[ 8] = 0.0f;
		d[ 9] = 0.0f;
		d[10] = 1.0f;
		d[11] = 0.0f;
	}

	/**
	 * Constructs a matrix by copying the one provided.
	 */
	public Matrix44af(Matrix44af m) {
		d = Arrays.copyOf(m.d, m.d.length);
	}

	/**
	 * Sets this matrix by copying the one provided.
	 */
	public void set(Matrix44af m) {
		System.arraycopy(m.d, 0, d, 0, d.length);
	}

	/**
	 * Constructs a matrix by copying the one provided.
	 */
	public Matrix44af(Matrix33f m) {
		d = new float[] { m.d[0], m.d[1], m.d[2], 0.0f, m.d[3], m.d[4], m.d[5], 0.0f, m.d[6], m.d[7], m.d[8], 0.0f };
	}

	/**
	 * Sets this matrix by copying the one provided.
	 */
	public void set(Matrix33f m) {
		d[ 0] = m.d[0];
		d[ 1] = m.d[1];
		d[ 2] = m.d[2];
		d[ 3] = 0.0f;
		d[ 4] = m.d[3];
		d[ 5] = m.d[4];
		d[ 6] = m.d[5];
		d[ 7] = 0.0f;
		d[ 8] = m.d[6];
		d[ 9] = m.d[7];
		d[10] = m.d[8];
		d[11] = 0.0f;
	}

	/**
	 * Constructs a matrix from the given elements, specified in row-major order.
	 */
	public Matrix44af(float m00, float m01, float m02, float m03, float m10, float m11, float m12, float m13, float m20, float m21, float m22, float m23) {
		d = new float[] { m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23 };
	}

	/**
	 * Sets this matrix from the given elements, specified in row-major order.
	 */
	public void set(float m00, float m01, float m02, float m03, float m10, float m11, float m12, float m13, float m20, float m21, float m22, float m23) {
		d[ 0] = m00;
		d[ 1] = m01;
		d[ 2] = m02;
		d[ 3] = m03;
		d[ 4] = m10;
		d[ 5] = m11;
		d[ 6] = m12;
		d[ 7] = m13;
		d[ 8] = m20;
		d[ 9] = m21;
		d[10] = m22;
		d[11] = m23;
	}

	/**
	 * Returns the given element using row-major ordering.
	 */
	public float get(int i) {
		return d[i];
	}

	/**
	 * Sets the given element using row-major ordering.
	 */
	public void set(int i, float v) {
		d[i] = v;
	}

	/**
	 * Returns the element at the given row and column.
	 */
	public float get(int row, int column) {
		return d[row*4 + column];
	}

	/**
	 * Sets the element at the given row and column.
	 */
	public void set(int row, int column, float v) {
		d[row*4 + column] = v;
	}

	/**
	 * Returns the sum of this matrix and {@code rhs}.
	 */
	public Matrix44af add(Matrix44af rhs) {
		return new Matrix44af(this).addAssign(rhs);
	}

	/**
	 * Adds {@code rhs} to this matrix.
	 */
	public Matrix44af addAssign(Matrix44af rhs) {
		for (int i = 0; i < d.length; ++i) {
			d[i] += rhs.d[i];
		}
		return this;
	}

	/**
	 * Returns the difference between this matrix and {@code rhs}.
	 */
	public Matrix44af subtract(Matrix44af rhs) {
		return new Matrix44af(this).subtractAssign(rhs);
	}

	/**
	 * Subtracts {@code rhs} from this matrix.
	 */
	public Matrix44af subtractAssign(Matrix44af rhs) {
		for (int i = 0; i < d.length; ++i) {
			d[i] -= rhs.d[i];
		}
		return this;
	}

	/**
	 * Returns this matrix negated.
	 */
	public Matrix44af negate() {
		return new Matrix44af(this).negateAssign();
	}

	/**
	 * Negates this matrix.
	 */
	public Matrix44af negateAssign() {
		for (int i = 0; i < d.length; ++i) {
			d[i] = -d[i];
		}
		return this;
	}

	/**
	 * Returns the product of this matrix and {@code rhs}.
	 */
	public Vector4f multiply(Vector4f rhs) {
		Vector4f vec = new Vector4f();
		for (int y = 0; y < 3; ++y) {
			float v = 0.0f;
			for (int x = 0; x < 4; ++x) {
				v += get(y, x) * rhs.get(x);
			}
			vec.set(y, v);
		}
		vec.set(3, rhs.get(3));
		return vec;
	}

	/**
	 * Returns the product of this matrix and {@code rhs}, where {@code rhs} has the implicit homogeneous coordinate 0.
	 */
	public Vector3f multiplyVector(Vector3f rhs) {
		Vector3f vec = new Vector3f();
		for (int y = 0; y < 3; ++y) {
			float v = 0.0f;
			for (int x = 0; x < 3; ++x) {
				v += get(y, x) * rhs.get(x);
			}
			vec.set(y, v);
		}
		return vec;
	}

	/**
	 * Returns the product of this matrix and {@code rhs}, where {@code rhs} has the implicit homogeneous coordinate 1.
	 */
	public Vector3f multiplyPoint(Vector3f rhs) {
		Vector3f vec = new Vector3f();
		for (int y = 0; y < 3; ++y) {
			float v = 0.0f;
			for (int x = 0; x < 3; ++x) {
				v += get(y, x) * rhs.get(x);
			}
			v += get(y, 3);
			vec.set(y, v);
		}
		return vec;
	}

	/**
	 * Returns the product of this matrix and {@code rhs}.
	 */
	public Matrix44af multiply(Matrix44af rhs) {
		return new Matrix44af(this).multiplyAssign(rhs);
	}

	/**
	 * Multiplies this matrix by {@code rhs}.
	 */
	public Matrix44af multiplyAssign(Matrix44af rhs) {
		float[] dt = new float[d.length];
		for (int y = 0; y < 3; ++y) {
			for (int x = 0; x < 3; ++x) {
				int idx = y*4 + x;
				for (int i = 0; i < 3; ++i) {
					dt[idx] += get(y, i) * rhs.get(i, x);
				}
			}
			// iteration for rightmost column
			int idx = y*4 + 3;
			for (int i = 0; i < 3; ++i) {
				dt[idx] += get(y, i) * rhs.get(i, 3);
			}
			dt[idx] += get(y, 3);
		}
		System.arraycopy(dt, 0, d, 0, d.length);
		return this;
	}

	/**
	 * Returns the product of this matrix and {@code rhs}.
	 */
	public Matrix44af multiply(Matrix33f rhs) {
		return new Matrix44af(this).multiplyAssign(rhs);
	}

	/**
	 * Multiplies this matrix by {@code rhs}.
	 */
	public Matrix44af multiplyAssign(Matrix33f rhs) {
		float[] dt = new float[d.length];
		for (int y = 0; y < 3; ++y) {
			for (int x = 0; x < 3; ++x) {
				int idx = y*4 + x;
				for (int i = 0; i < 3; ++i) {
					dt[idx] += get(y, i) * rhs.get(i, x);
				}
			}
			int idx = y*4 + 3;
			dt[idx] = get(y, 3);
		}
		System.arraycopy(dt, 0, d, 0, d.length);
		return this;
	}

	/**
	 * Pre-multiplies this matrix by {@code lhs}.
	 */
	public Matrix44af preMultiplyAssign(Matrix44af lhs) {
		float[] dt = new float[d.length];
		for (int y = 0; y < 3; ++y) {
			for (int x = 0; x < 3; ++x) {
				int idx = y*4 + x;
				for (int i = 0; i < 3; ++i) {
					dt[idx] += lhs.get(y, i) * get(i, x);
				}
			}
			// iteration for rightmost column
			int idx = y*4 + 3;
			for (int i = 0; i < 3; ++i) {
				dt[idx] += lhs.get(y, i) * get(i, 3);
			}
			dt[idx] += lhs.get(y, 3);
		}
		System.arraycopy(dt, 0, d, 0, d.length);
		return this;
	}

	/**
	 * Pre-multiplies this matrix by {@code lhs}.
	 */
	public Matrix44af preMultiplyAssign(Matrix33f lhs) {
		float[] dt = new float[d.length];
		for (int y = 0; y < 3; ++y) {
			for (int x = 0; x < 4; ++x) {
				int idx = y*4 + x;
				for (int i = 0; i < 3; ++i) {
					dt[idx] += lhs.get(y, i) * get(i, x);
				}
			}
		}
		System.arraycopy(dt, 0, d, 0, d.length);
		return this;
	}

	/**
	 * Returns the product of this matrix and {@code rhs}.
	 */
	public Matrix44af multiply(float rhs) {
		return new Matrix44af(this).multiplyAssign(rhs);
	}

	/**
	 * Multiplies this matrix by {@code rhs}.
	 */
	public Matrix44af multiplyAssign(float rhs) {
		for (int i = 0; i < d.length; ++i) {
			d[i] *= rhs;
		}
		return this;
	}

	/**
	 * Returns the quotient of this matrix and {@code rhs}.
	 */
	public Matrix44af divide(float rhs) {
		return new Matrix44af(this).divideAssign(rhs);
	}

	/**
	 * Divides this matrix by {@code rhs}.
	 */
	public Matrix44af divideAssign(float rhs) {
		for (int i = 0; i < d.length; ++i) {
			d[i] /= rhs;
		}
		return this;
	}

	/**
	 * Returns whether this matrix is equal to the object {@code o}.
	 */
	@Override
	public boolean equals(Object o) {
		if (o == this) {
			return true;
		} else if (o == null || !(o instanceof Matrix44af)) {
			return false;
		} else {
			return equals((Matrix44af) o);
		}
	}

	/**
	 * Returns a hash code for this object.
	 */
	@Override
	public int hashCode() {
		int bits = 0;
		for (int i = 0; i < d.length; ++i) {
			bits ^= Float.floatToIntBits(d[i]);
		}
		return bits;
	}

	/**
	 * Returns whether this matrix is equal to the matrix {@code rhs}.
	 */
	public boolean equals(Matrix44af rhs) {
		return Arrays.equals(d, rhs.d);
	}

	/**
	 * Returns the inverse of this matrix, or {@code null} if this matrix is not invertible.
	 */
	public Matrix44af invert() {
		return new Matrix44af(this).invertAssign();
	}

	/**
	 * Inverts this matrix, or returns {@code null} if this matrix is not invertible.
	 */
	public Matrix44af invertAssign() {
		float det = determinant();
		if (det == 0.0f) {
			return null;
		}

		float invDet = 1.0f / det;
		float[] dt = new float[] {
				(d[ 5]*d[10] - d[ 6]*d[ 9]) * invDet,
				(d[ 2]*d[ 9] - d[ 1]*d[10]) * invDet,
				(d[ 1]*d[ 6] - d[ 2]*d[ 5]) * invDet,
				(d[ 1]*d[ 7]*d[10] + d[ 2]*d[ 5]*d[11] + d[ 3]*d[ 6]*d[ 9] - d[ 1]*d[ 6]*d[11] - d[ 2]*d[ 7]*d[ 9] - d[ 3]*d[ 5]*d[10]) * invDet,
				(d[ 6]*d[ 8] - d[ 4]*d[10]) * invDet,
				(d[ 0]*d[10] - d[ 2]*d[ 8]) * invDet,
				(d[ 2]*d[ 4] - d[ 0]*d[ 6]) * invDet,
				(d[ 0]*d[ 6]*d[11] + d[ 2]*d[ 7]*d[ 8] + d[ 3]*d[ 4]*d[10] - d[ 0]*d[ 7]*d[10] - d[ 2]*d[ 4]*d[11] - d[ 3]*d[ 6]*d[ 8]) * invDet,
				(d[ 4]*d[ 9] - d[ 5]*d[ 8]) * invDet,
				(d[ 1]*d[ 8] - d[ 0]*d[ 9]) * invDet,
				(d[ 0]*d[ 5] - d[ 1]*d[ 4]) * invDet,
				(d[ 0]*d[ 7]*d[ 9] + d[ 1]*d[ 4]*d[11] + d[ 3]*d[ 5]*d[ 8] - d[ 0]*d[ 5]*d[11] - d[ 1]*d[ 7]*d[ 8] - d[ 3]*d[ 4]*d[ 9]) * invDet
		};
		System.arraycopy(dt, 0, d, 0, d.length);
		return this;
	}

	/**
	 * Returns this matrix rotated by {@code a} radians about the x-axis.
	 */
	public Matrix44af rotateX(float a) {
		return new Matrix44af(this).rotateXAssign(a);
	}

	/**
	 * Rotates this matrix by {@code a} radians about the x-axis.
	 */
	public Matrix44af rotateXAssign(float a) {
		float sinA = (float) Math.sin(a);
		float cosA = (float) Math.cos(a);

		float r11 = cosA;
		float r12 = -sinA;
		float r21 = sinA;
		float r22 = cosA;

		float m10 = r11*d[4] + r12*d[ 8];
		float m11 = r11*d[5] + r12*d[ 9];
		float m12 = r11*d[6] + r12*d[10];
		float m13 = r11*d[7] + r12*d[11];

		float m20 = r21*d[4] + r22*d[ 8];
		float m21 = r21*d[5] + r22*d[ 9];
		float m22 = r21*d[6] + r22*d[10];
		float m23 = r21*d[7] + r22*d[11];

		d[4] = m10; d[5] = m11; d[ 6] = m12; d[ 7] = m13;
		d[8] = m20; d[9] = m21; d[10] = m22; d[11] = m23;

		return this;
	}

	/**
	 * Returns this matrix rotated by {@code a} radians about the y-axis.
	 */
	public Matrix44af rotateY(float a) {
		return new Matrix44af(this).rotateYAssign(a);
	}

	/**
	 * Rotates this matrix by {@code a} radians about the y-axis.
	 */
	public Matrix44af rotateYAssign(float a) {
		float sinA = (float) Math.sin(a);
		float cosA = (float) Math.cos(a);

		float r00 = cosA;
		float r02 = sinA;
		float r20 = -sinA;
		float r22 = cosA;

		float m00 = r00*d[0] + r02*d[ 8];
		float m01 = r00*d[1] + r02*d[ 9];
		float m02 = r00*d[2] + r02*d[10];
		float m03 = r00*d[3] + r02*d[11];

		float m20 = r20*d[0] + r22*d[ 8];
		float m21 = r20*d[1] + r22*d[ 9];
		float m22 = r20*d[2] + r22*d[10];
		float m23 = r20*d[3] + r22*d[11];

		d[0] = m00; d[1] = m01; d[ 2] = m02; d[ 3] = m03;
		d[8] = m20; d[9] = m21; d[10] = m22; d[11] = m23;

		return this;
	}

	/**
	 * Returns this matrix rotated by {@code a} radians about the z-axis.
	 */
	public Matrix44af rotateZ(float a) {
		return new Matrix44af(this).rotateZAssign(a);
	}

	/**
	 * Rotates this matrix by {@code a} radians about the z-axis.
	 */
	public Matrix44af rotateZAssign(float a) {
		float sinA = (float) Math.sin(a);
		float cosA = (float) Math.cos(a);

		float r00 = cosA;
		float r01 = -sinA;
		float r10 = sinA;
		float r11 = cosA;

		float m00 = r00*d[0] + r01*d[4];
		float m01 = r00*d[1] + r01*d[5];
		float m02 = r00*d[2] + r01*d[6];
		float m03 = r00*d[3] + r01*d[7];

		float m10 = r10*d[0] + r11*d[4];
		float m11 = r10*d[1] + r11*d[5];
		float m12 = r10*d[2] + r11*d[6];
		float m13 = r10*d[3] + r11*d[7];

		d[0] = m00; d[1] = m01; d[2] = m02; d[3] = m03;
		d[4] = m10; d[5] = m11; d[6] = m12; d[7] = m13;

		return this;
	}

	/**
	 * Returns this matrix rotated about the given axis by the given angle.
	 */
	public Matrix44af rotate(AxisAngle aa) {
		return new Matrix44af(this).rotateAssign(aa);
	}

	/**
	 * Rotates this matrix about the given axis by the given angle.
	 */
	public Matrix44af rotateAssign(AxisAngle aa) {
		Vector3f axis = aa.axis;
		float sinA = (float) Math.sin(aa.angle);
		float cosA = (float) Math.cos(aa.angle);
		float xy = axis.x()*axis.y();
		float xz = axis.x()*axis.z();
		float yz = axis.y()*axis.z();

		Matrix33f rot = new Matrix33f(
				cosA + axis.x()*axis.x()*(1 - cosA),
				xy*(1 - cosA) - axis.z()*sinA,
				xz*(1 - cosA) + axis.y()*sinA,
				xy*(1 - cosA) + axis.z()*sinA,
				cosA + axis.y()*axis.y()*(1 - cosA),
				yz*(1 - cosA) - axis.x()*sinA,
				xz*(1 - cosA) - axis.y()*sinA,
				yz*(1 - cosA) - axis.x()*sinA,
				cosA + axis.z()*axis.z()*(1 - cosA));
		return preMultiplyAssign(rot);
	}

	/**
	 * Returns this matrix rotated by the given quaternion.
	 */
	public Matrix44af rotate(Quaternionf q) {
		return new Matrix44af(this).rotateAssign(q);
	}

	/**
	 * Rotates this matrix by the given quaternion.
	 */
	public Matrix44af rotateAssign(Quaternionf q) {
		float i2 = 2.0f*q.i();
		float j2 = 2.0f*q.j();
		float k2 = 2.0f*q.k();

		Matrix33f rot = new Matrix33f(
				i2*q.i(),
				i2*q.j(),
				i2*q.k(),
				i2*q.r(),
				j2*q.j(),
				j2*q.k(),
				j2*q.r(),
				k2*q.k(),
				k2*q.r());
		return preMultiplyAssign(rot);
	}

	/**
	 * Returns this matrix scaled by {@code s}.
	 */
	public Matrix44af scale(Vector3f s) {
		return new Matrix44af(this).scaleAssign(s);
	}

	/**
	 * Scales this matrix by {@code s}.
	 */
	public Matrix44af scaleAssign(Vector3f s) {
		d[ 0] *= s.get(0);
		d[ 1] *= s.get(0);
		d[ 2] *= s.get(0);
		d[ 3] *= s.get(0);
		d[ 4] *= s.get(1);
		d[ 5] *= s.get(1);
		d[ 6] *= s.get(1);
		d[ 7] *= s.get(1);
		d[ 8] *= s.get(2);
		d[ 9] *= s.get(2);
		d[10] *= s.get(2);
		d[11] *= s.get(2);
		return this;
	}

	/**
	 * Returns this matrix translated by {@code t}.
	 */
	public Matrix44af translate(Vector3f t) {
		return new Matrix44af(this).translateAssign(t);
	}

	/**
	 * Translates this matrix by {@code t}.
	 */
	public Matrix44af translateAssign(Vector3f t) {
		d[3] += t.get(0);
		d[7] += t.get(1);
		d[8] += t.get(2);
		return this;
	}
	/**
	 * Returns the determinant of this matrix.
	 */
	public float determinant() {
		return d[0]*(d[5]*d[10] - d[6]*d[9]) - d[1]*(d[4]*d[10] - d[6]*d[8]) + d[2]*(d[4]*d[ 9] - d[5]*d[8]);
	}

	/**
	 * Returns the trace of this matrix.
	 */
	public float trace() {
		return d[0] + d[5] + d[10] + 1.0f;
	}

	/**
	 * Returns the raw underlying mutable array.
	 */
	public float[] getArray() {
		return d;
	}

	/**
	 * Converts this matrix to a 4x4 matrix suitable for graphics transformations.
	 */
	public Matrix44f getGraphicsMatrix() {
		return new Matrix44f(
				d[0], d[1], d[ 2], d[ 3],
				d[4], d[5], d[ 6], d[ 7],
				d[8], d[9], d[10], d[11],
				0.0f, 0.0f, 0.0f,  1.0f);
	}
}
